// -*- coding: utf-8 -*-

#include <iostream>
#include <fstream>
#include <cmath>
#include <boost/shared_array.hpp>

#include "elisa/elisa_interval.h"
#include "hybrid_solver.h"
#include "hybrid_newton_reduction.h"
#include "hybrid_util.h"

using namespace vnodelp;
using namespace v_bias;
using namespace v_blas;

USING_ELISA

//--------------------------------------------------------------------------
// class HybridReduction
//--------------------------------------------------------------------------

/*
const double HybridNewtonReduction::MinH(1.0e-10);
//const double HybridNewtonReduction::MinH(1.0e-13);
const int    HybridNewtonReduction::Ratio(10);
*/
const double HybridNewtonReduction::Factor(1.0-1.0e-1);

//bool HybridNewtonReduction::dump(false);

ContConstraintRep *rep;

bool HybridNewtonReduction::reductionAlgorithm()
{
#ifdef HYBRID_PROFILE
	++numHybridReduction;
#endif

	// 必要な情報を集める
	//ContConstraintRep *rep = m_constr.rep();
	rep = m_constr.rep();
	Time&      time = rep->getTime();
	TimeImpl  *ti   = dynamic_cast<TimeImpl *>(time.getImplementation().get());
	// 時間ドメイン
	VNInterval td(ti->domain().inf(), ti->domain().sup());

	// 連続状態ドメイン (を入れる変数)
	VNIntVec   aX;

#ifdef _DEBUG
	cout << "narrowing... ";
#endif

#ifndef FIRST_SOL_ONLY
	// 左側を削減
	if (!narrow(true,  td, aX))
		return false;
	// 右側を削減
	if (!narrow(false, td, aX))
		return false;

#else // FIRST_SOL_ONLY

#ifdef TRY_LO
	bool reset(false);
start_solving:
#endif

	// 初期状態の集合
	VNIntVec   x(m_odeSolver.getInitValue());
	// 初期時間の集合
	VNInterval minT(m_odeSolver.getInitTime());

	// 時間ドメインの下端の状態を求める
	double maxT = inf(td);
	double offset = maxT - sup(minT);
	if (offset > 0) {
		x = m_odeSolver.getEnclosure(maxT);
		minT = maxT;
	}

	double wx0(v_bias::width(x[0]) > 0 ? v_bias::width(x[0]) : MinH);

	//vnodelp::VNODE vn(m_odeSolver.getVnode());
	vnodelp::VNODE vn(m_constr.rep()->getAD().get());

	maxT = sup(td);
	vn.setFirstEntry();
	vn.setHmax(maxT-inf(minT));
	vn.setOneStep(on);
	bool leftNrw(false);
#ifdef FIRST_IS_SECOND
	static bool first(true);
#endif
	while (true) {
		if (sup(minT) >= maxT)
			return false;

		// 1ステップだけ計算する。
		//vn.setFirstEntry();
		vn.integrate(minT, x, maxT);

#ifdef TRY_LO
		// 区間爆発が起こっていたら設定を変えてやり直す
		if (!reset && v_bias::width(x[0]) > wx0*Factor) {
			reset = true;
			m_odeSolver.setupLO();
			goto start_solving;
		}
#endif

		VNInterval td1(vn.getT());
		VNIntVec   aX1(vn.getAprioriEncl());

#ifdef HYBRID_DUMP
		if (HybridOdeSolver::dump) dumpPipe(nrwOut, m_constr, td1, aX1);
#endif

		if (!leftNrw) {
			td = td1;
			aX = aX1;
#ifdef PIPE_CACHING
			VNEncl encl(VNIVPtr(new VNIntVec(aX)), VNIVPtr(new VNIntVec(x)));
			m_odeSolver.savePipe(VNPipe(VNIntPtr(new VNInterval(td)), encl));
#endif
		} else {
			td = vn::hull(td, td1);
			transform(aX.begin(), aX.end(), aX1.begin(), aX.begin(), vn::hull);
		}

		if (!leftNrw)
			// 左側を削減
			if (narrow(true,  td, aX))
				leftNrw = true;

		if (leftNrw) {
			// 右側を削減
			double rb(sup(td));
			if (narrow(false,  td, aX)) {
				// はじめから十分に狭かった
				if (sup(td) == rb && width(td) <= time.getPrecision()) 
					break;
		
				// 削減された
				if (sup(td) < rb) {
#ifdef FIRST_IS_SECOND
					if (first) {
						first = false;
					} else
#endif
					break;
				}
			} else
				// ありえない
				throw("HNW failed!");
		}
	}
#endif // FIRST_SOL_ONLY

#ifdef _DEBUG
	// 削減結果の出力
	cout << "finished." << endl << "t: " << td << endl;
#endif

	ti->setDomain(td);

	// ODE求解結果をElisaドメインに伝播する
	const ContVarVec & cvv = rep->getContVars();
	for (int i(0); i < cvv.size(); ++i) {
		ContVarImpl *cv = dynamic_cast<ContVarImpl *>(cvv[i]->getImplementation().get());

		Interval d(aX[i]);
		d.intersect(cv->domain());
		if (d.isEmpty())
			return false;
		cv->setDomain(d);
	}

	return true;
}


// 左側あるいは右端を削減する
bool HybridNewtonReduction::narrow(bool left, VNInterval& t, VNIntVec& aX)
{
#ifdef HYBRID_PROFILE
	++numNarrow;
#endif

	// ガード条件の判定
//#ifndef FIRST_SOL_ONLY
#ifdef _DEBUG
cout << "encl: " << t << ':';
#endif
	aX = m_odeSolver.getEnclosure(t);
#ifdef _DEBUG
cout << "aX: " << aX[0] << endl;
#endif
#ifdef HYBRID_DUMP
	if (HybridOdeSolver::dump) dumpPipe(nrwOut, m_constr, t, aX);
#endif
//#endif // FIRST_SOL_ONLY

	// ドメインとの積をとる
    const ContVarVec & cvv = rep->getContVars();
    for (int i(0); i < cvv.size(); ++i) {
        ContVarImpl *cv = dynamic_cast<ContVarImpl *>(cvv[i]->getImplementation().get());

        v_bias::intersect(aX[i], aX[i], 
			VNInterval(cv->domain().inf(), cv->domain().sup()));
    }

	if (!m_constr.getGFunc()(aX, t).set_contains(VNInterval::zero))
		return false;

	VNInterval t1(t);
#ifdef USE_INEWTON
	// tを削減する
	if (!newtonIt(t1, aX))
		return false;

#ifdef _DEBUG
	cout << "newt: " << t1 << endl;
#endif
#ifdef HYBRID_DUMP
	if (HybridOdeSolver::dump) dumpPipe(nrwOut, m_constr, t1, aX);
#endif
#endif

	// 20090907
	if (inf(t1) == 0.0 && width(t1) <= MinH*Ratio)
		return false;

	// 端から最短幅で解く
	VNInterval t2;
	VNIntVec aX1;
	m_odeSolver.getBBox(left, t1, t2, aX1);

	// 端でのガード条件の評価
	if (m_constr.getGFunc()(aX1, t2).set_contains(VNInterval::zero) 
		|| width(t1) <= MinH*Ratio) {

			if (left) {
				t = vn::hull(t1, sup(t));
			} else {
				t = vn::hull(inf(t), t1);
				//t = t1;
			}

	} else {
		//++bc;

		// tを分割する
		VNInterval l, r;
		t1.split(l, r);

		//aX1 = aX;
		if (left) {
			if (narrow(true, l, aX1)) { // L-L
				t = vn::hull(l, sup(t));
				//aX = aX1;

			} else { // L-R

				//aX1 = aX;
				if (narrow(true, r, aX1)) {
					t = vn::hull(r, sup(t));
				} else
					return false;
			}
		} else {

			if (narrow(false, r, aX1)) { // R-R
				t = vn::hull(inf(t), r);
				//aX = aX1;

			} else { // R-L

				//aX1 = aX;
				if (narrow(false, l, aX1)) {
					t = vn::hull(inf(t), l);

				} else	
					// 不要？
					return false;
			}
		}
	}
	return true;
}

// 不動点に達するまでニュートン法を適用する
bool HybridNewtonReduction::newtonIt(VNInterval& t, VNIntVec& aX)
{
	VNInterval nextT(t);

	double		wT(m_odeSolver.getInitTime().width());

	double		midT;
	VNIntVec	midX;

	while (!nextT.is_empty() && width(nextT) - wT > MinH*(Ratio)) {
#ifdef HYBRID_PROFILE
		++numNewton;
#endif

		aX = m_odeSolver.getEnclosure(nextT);
		m_odeSolver.getMEncl(nextT, midT, midX);

		// 削減前のTを保存
		t = nextT;

		// 区間ニュートン法によるTの削減
		VNInterval h(m_constr.getGFunc()(midX, midT));
		VNInterval dh(0.0);

		{
			const int n(aX.size());

			// 配列
			boost::shared_array<VNInterval> axa(new VNInterval[n]);
			// 自動微分用変数ベクトル
			AD_VNIntVec adv(n);

			for (int i(0); i < aX.size(); i++) {
				axa[i] = aX[i];
				adv[i] = AD_VNInterval(aX[i]);
			}

			// 時間
			AD_VNInterval adt(t);

			// 自動微分用DAGを作成
			AD_VNInterval gfunc(m_constr.getDGFunc()(adv, adt));
			gfunc.diff(0, 1);

			// 軌道の微分値を計算
			boost::shared_array<VNInterval> dtraj(new VNInterval[n]);
			m_constr.getDFunc()(n, dtraj.get(), axa.get(), aX[0], NULL);

			dh = adt.d(0);
			for (int i(0); i < adv.size(); i++) {
				dh += adv[i].d(0) * dtraj[i];
			}
		}

		if (dh.set_contains(0.0))
			return true;

		v_bias::intersect(nextT, nextT, (midT - h/dh));

		bool ue(false);
		if (t.set_strictly_contains(nextT)) {
			ue = true;
			ueEncls.push_back(t);
		}

#ifdef _DEBUG
		// 削減結果の出力
		cout << "t0: " << t << endl;
		cout << "t:  " << nextT << endl;
		cout << "ue: " << ue << endl;
#endif

		// 20090325
		// 削減しすぎた場合は少し増やす
		// TODO: 存在保証ができた場合は不要？
		if (width(nextT) < MinH*Ratio) {
			nextT = VNInterval(inf(nextT), inf(nextT)+MinH*Ratio);
			//return true;
		}

		// 十分削減された
		if (width(nextT) >= width(t)*Factor)
			break;
	}

	// 解なし
	if (nextT.is_empty())
		return false;

	t = nextT;
	return true;
}


void elisa::ueReset() {
	ueEncls.clear();
}

bool elisa::ueEncl(const VNInterval& encl) {
	VNIntVec::iterator it(ueEncls.begin());
	for (; it != ueEncls.end(); ++it) {
		if (v_bias::subseteq(encl, *it)) {
			return true;
		}
	}
	return false;
}
